diff options
Diffstat (limited to 'src/core/hid/motion_input.cpp')
| -rw-r--r-- | src/core/hid/motion_input.cpp | 21 |
1 files changed, 20 insertions, 1 deletions
diff --git a/src/core/hid/motion_input.cpp b/src/core/hid/motion_input.cpp index b60478dbb..f56f2ae1d 100644 --- a/src/core/hid/motion_input.cpp +++ b/src/core/hid/motion_input.cpp @@ -37,11 +37,17 @@ void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) { gyro.y = std::clamp(gyro.y, -GyroMaxValue, GyroMaxValue); gyro.z = std::clamp(gyro.z, -GyroMaxValue, GyroMaxValue); - // Auto adjust drift to minimize drift + // Auto adjust gyro_bias to minimize drift if (!IsMoving(IsAtRestRelaxed)) { gyro_bias = (gyro_bias * 0.9999f) + (gyroscope * 0.0001f); } + // Adjust drift when calibration mode is enabled + if (calibration_mode) { + gyro_bias = (gyro_bias * 0.99f) + (gyroscope * 0.01f); + StopCalibration(); + } + if (gyro.Length() < gyro_threshold * user_gyro_threshold) { gyro = {}; } else { @@ -107,6 +113,19 @@ void MotionInput::UpdateRotation(u64 elapsed_time) { rotations += gyro * sample_period; } +void MotionInput::Calibrate() { + calibration_mode = true; + calibration_counter = 0; +} + +void MotionInput::StopCalibration() { + if (calibration_counter++ > CalibrationSamples) { + calibration_mode = false; + ResetQuaternion(); + ResetRotations(); + } +} + // Based on Madgwick's implementation of Mayhony's AHRS algorithm. // https://github.com/xioTechnologies/Open-Source-AHRS-With-x-IMU/blob/master/x-IMU%20IMU%20and%20AHRS%20Algorithms/x-IMU%20IMU%20and%20AHRS%20Algorithms/AHRS/MahonyAHRS.cs void MotionInput::UpdateOrientation(u64 elapsed_time) { |